#include <iostream>
#include <chrono>
#include <vector>
#include <string>
#include <fstream>
#include <sstream>
#include <opencv2/opencv.hpp>
#include "superpoint.h"
#include "lightglue.h"

using namespace std;
using namespace cv;

int main(){
    const string sp_onnx_path = "/zwj/pro/zwl/Feature/LightGlue-ONNX/weights/superpoint.onnx";
    const string lg_onnx_path = "/zwj/pro/zwl/Feature/LightGlue-ONNX/weights/superpoint_lightglue.onnx";

    std::string folderPath = "/ws/zwl/Data/test_relocalization_data/gly_part/euroc/cam0/data/"; // 替换为实际文件夹的路径
    std::ifstream file("/ws/zwl/Data/test_relocalization_data/gly_part/euroc/cam0/data.csv");
    if (!file.is_open()) {
        std::cerr << "Failed to open CSV file." << std::endl;
        return 1;
    }
    std::vector<std::string> img_paths;
    std::string line;
    std::getline(file, line);
    while(std::getline(file, line)){
        std::istringstream lineStream(line);
        std::string cell;
        // 移动到第二列（文件名列）
        std::getline(lineStream, cell, ','); // 假设CSV文件以逗号分隔，这获取第一列
        std::getline(lineStream, cell, ','); // 获取第二列，即文件名列
        img_paths.push_back(folderPath+cell);
    } 

    auto img1_size = cv::Size(1920, 960);
    auto img2_size = cv::Size(1920, 960);
    auto sp = SuperPoint(sp_onnx_path, 1);
    auto lg = LightGlue("LG", lg_onnx_path, img1_size, img2_size, 1);

    std::string outputVideoPath = "/ws/zwl/Data/test_relocalization_data/gly_part/euroc/cam0/light_match.mp4"; // 输出视频文件的路径
    int frameWidth = 1920; // 帧宽度
    int frameHeight = 1920; // 帧高度
    int frameRate = 30; // 帧率（每秒帧数）
    cv::VideoWriter videoWriter(outputVideoPath, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'), frameRate, cv::Size(frameWidth, frameHeight));
    if (!videoWriter.isOpened()) {
        std::cerr << "Failed to open video writer." << std::endl;
        return 1;
    }



    for(int i=0; i<img_paths.size()-1; ++i){
        auto img1_path = img_paths[i];
        auto img2_path = img_paths[i+1];

        std::vector<cv::KeyPoint> keypoints1, keypoints2;
	    cv::Mat descriptors1, descriptors2;
        cv::Mat img1 = cv::imread(img1_path);
        cv::Mat img2 = cv::imread(img2_path);
        cv::Mat resize_img1, resize_img2;
        cv::resize(img1, resize_img1, cv::Size(1920, 960));
        cv::resize(img2, resize_img2, cv::Size(1920, 960));
        sp.DetectAndCompute(resize_img1, keypoints1, descriptors1);
        sp.DetectAndCompute(resize_img2, keypoints2, descriptors2);
        auto match_res = lg.L_Match(keypoints1, keypoints2, descriptors1, descriptors2);
    
        //画出匹配的特征点
        cv::Mat img_matches;
        //拼接两幅图像作为画布
        cv::vconcat(resize_img1, resize_img2, img_matches);
        for(int i=0;i<match_res.size();i++)
        {
            cv::Point2f pt1=keypoints1[match_res[i].queryIdx].pt;
            cv::Point2f pt2=keypoints2[match_res[i].trainIdx].pt;
            //特征点坐标需要根据图像偏移量进行修正
            pt2.y+=resize_img1.rows;
            cv::line(img_matches,pt1,pt2,cv::Scalar(0,255,0),2);   
        }
        videoWriter.write(img_matches);
    }
    videoWriter.release();

}

